// Copyright 2008 Isis Innovation Limited
#include "MapMaker.h"
#include "MapPoint.h"
#include "Bundle.h"
#include "PatchFinder.h"
#include "HomographyInit.h"
#include "RTree.h"
#include "Bloomenthal.h"
#include "RandomHelpers.h"

#include <cvd/vector_image_ref.h>
#include <cvd/vision.h>
#include <cvd/image_interpolate.h>

#include <TooN/SVD.h>
#include <TooN/SymEigen.h>

#include <gvars3/instances.h>
#include <fstream>
#include <iostream>
#include <algorithm>
#include <limits>

// Metric Tree includes
#include <arboretum/stMetricTree.h>
#include <arboretum/stPlainDiskPageManager.h>
#include <arboretum/stDiskPageManager.h>
#include <arboretum/stMemoryPageManager.h>
#include <arboretum/stMetricTree.h>
#include <arboretum/stMetricAccessMethod.h>
#include <arboretum/stFMMTree.h>
#include <arboretum/stUserLayerUtil.h>

#ifdef WIN32
#define WIN32_LEAN_AND_MEAN
#include <windows.h>
#endif

using namespace CVD;
using namespace std;
using namespace GVars3;

// Constructor sets up internal reference variable to Map.
// Most of the intialisation is done by Reset()..
MapMaker::MapMaker(const ATANCamera &cam, Scarf &s) 
  : mCamera(cam), 
    mScarf(s),
    mPoseEstimator(cam)
{
  mMap = new Map;
  mbResetRequested = false;
  mdLargestRadiusInImage = mCamera.LargestRadiusInImage() * mCamera.LargestRadiusInImage();
  mv2ImageSize = mCamera.GetImageSize();

  mResetType = COMPLETE;
  Reset();

  start(); // This CVD::thread func starts the map-maker thread with function run()
  GUI.RegisterCommand("SaveMap", GUICommandCallBack, this);
  GUI.RegisterCommand("TogglePause", GUICommandCallBack, this);
  GV3::Register(mgvdKeyFrameSpacing, "MapMaker.KeyFrameSpacing", 0.5, SILENT); // Target metric distance between KeyFrames
  GV3::Register(mgvnMinMergeSize, "MapMaker.MinMergeSize", 50, SILENT);
  GV3::Register(mgvnMinLocalKeyFrames, "MapMaker.MinLocalKeyFrames", 35, SILENT); // Default to target of 150 keyframes for local map
  GV3::Register(mgvnTargetLocalKeyFrames, "MapMaker.TargetLocalKeyFrames", 150, SILENT); // Default to target of 150 keyframes for local map
  GV3::Register(mgvnTargetLocalPoints, "MapMaker.TargetLocalPoints", 6000, SILENT); // Default to target of 6000 points for local map
  GV3::Register(mgvdMaxSceneDepth, "MapMaker.MaxSceneDepth", 2.0, SILENT); // Max expected distance from (locally) planar scene approximation
  GV3::Register(mgvdMaxSceneAngle, "MapMaker.MaxSceneAngle", 0.34906585, SILENT); // Max expected angle from (locally) planar scene normal (20deg=0.34906585rad)
  GV3::Register(mgvnRansacMinObservations, "MapMaker.RansacMinObservations", 30, SILENT);
  GV3::Register(mgvnRansacMaxHypotheses, "MapMaker.RansacMaxHypotheses", 250, SILENT);
  GV3::Register(mgvnRansacBlocks, "MapMaker.RansacBlocks", 5, SILENT);
  GV3::Register(mgvnRansacBlockSize, "MapMaker.RansacBlockSize", 100, SILENT);
  GV3::Register(mgvdMaxRotationError, "System.MaxRotationError", 3.0, SILENT);
  GV3::Register(mgvdMaxTranslationError, "System.MaxTranslationError", 3.0, SILENT);
  GV3::Register(mgvnGlobalSE3, "System.GlobalSE3", 0, SILENT);
  GV3::Register(mgvnGlobalSO3, "System.GlobalSO3", 0, SILENT);

  // Cache gvars for the new map.
  mdKeyFrameSpacing = *mgvdKeyFrameSpacing;
  mnMinMergeSize = *mgvnMinMergeSize;
  mnMinLocalKeyFrames = *mgvnMinLocalKeyFrames;
  mnTargetLocalKeyFrames = *mgvnTargetLocalKeyFrames;
  mnTargetLocalPoints = *mgvnTargetLocalPoints;
  mdMaxSceneDepth = *mgvdMaxSceneDepth;
  mdMaxOverlapDistance = mdMaxSceneDepth * atan(mCamera.GetFOVs()[2] + *mgvdMaxSceneAngle);
  mnRansacMinObservations = *mgvnRansacMinObservations;  // Cache gvar.
  mnRansacMaxHypotheses = *mgvnRansacMaxHypotheses;  // Cache gvar.
  mnRansacBlocks = *mgvnRansacBlocks;  // Cache gvar.
  mnRansacBlockSize = *mgvnRansacBlockSize;  // Cache gvar.
  mdMaxRotationError = *mgvdMaxRotationError;
  mdMaxTranslationError = *mgvdMaxTranslationError;
  mnGlobalSE3 = *mgvnGlobalSE3;
  mnGlobalSO3 = *mgvnGlobalSO3;

};

void MapMaker::Reset()
{
  // This is only called from within the mapmaker thread...
  if(mResetType == COMPLETE)
  {
std::cout << "Complete reset" << std::endl;
    for(unsigned int i=0; i<mvOldMaps.size(); i++)
      delete mvOldMaps[i];
    mvOldMaps.clear();
    mMap->Reset();

    if(ofsLogger.is_open())
      ofsLogger.close();
    ofsLogger.open("MapMaker.log", ios_base::trunc);
  } 
  else if(mResetType == NEW_MAP)
  {
std::cout << "New map reset " << mvOldMaps.size() << std::endl;
    // Keep the current map if it is reasonably sized...
    if(mMap->mrtAllKeyFrames.Size() > mnMinMergeSize)
    {
      std::cout << "Saved old map" << std::endl;
      mvOldMaps.push_back(mMap);
      mMap = new Map();
    }
    else
      mMap->Reset();
  }
  else //mResetType == PREV_MAP
  {
std::cout << "Prev map reset " << mvOldMaps.size() << std::endl;
    if(!mvOldMaps.empty()) // If there was a previous map revert back to last
    {
      std::cout << "Restored old map" << std::endl;
      delete mMap;
      mMap = mvOldMaps.back();
      mvOldMaps.pop_back();
    }
    else  // Otherwise just a complete reset...
      mMap->Reset();
  }

  mpMostRecentKF.reset();
  mvFailureQueue.clear();
  mvNewQueue.clear();

  mvpKeyFrameQueue.clear();
  mbBundleRunning = false;
  mbBundleConverged_Full = true;
  mbBundleConverged_Recent = true;
  mbResetDone = true;
  mbMapMakerPaused = false;
  mbResetRequested = false;
  mResetType = COMPLETE;
  mbBundleAbortRequested = false;
  mnBundleNotConvergedCount = 0;
  mbUpdateLocalMap = false;
}

// CHECK_RESET is a handy macro which makes the mapmaker thread stop
// what it's doing and reset, if required.
#define CHECK_RESET if(mbResetRequested) {Reset(); continue;};

void MapMaker::run()
{
#ifdef WIN32
  // For some reason, I get tracker thread starvation on Win32 when
  // adding key-frames. Perhaps this will help:
  SetThreadPriority(GetCurrentThread(), THREAD_PRIORITY_LOWEST);
#endif
    std::cout << "Comienza bucle mapmaker" << std::endl;
  while(!shouldStop())  // ShouldStop is a CVD::Thread func which return true if the thread is told to exit.
  {
    //   std::cout << "Bucle MapMaker" << std::endl;
    CHECK_RESET;
    //sleep(5); // Sleep not really necessary, especially if mapmaker is busy
    CHECK_RESET;
    //  std::cout << "33" << std::endl;
    // Handle any GUI commands encountered..
    while(!mvQueuedCommands.empty())
    {
      //   std::cout << "1" << std::endl;
      GUICommandHandler(mvQueuedCommands.begin()->sCommand, mvQueuedCommands.begin()->sParams);
      mvQueuedCommands.erase(mvQueuedCommands.begin());
    }
    
    if(!mMap->IsGood() || mbMapMakerPaused)
    {// Nothing to do if there is no map yet!
    //  std::cout << "99" << std::endl;
      continue;
    }
    // From here on, mapmaker does various map-maintenance jobs in a certain priority
    // Hierarchy. For example, if there's a new key-frame to be added (QueueSize() is >0)
    // then that takes high priority.

    CHECK_RESET;
    if(mbUpdateLocalMap)
    {
      //   std::cout << "2" << std::endl;
      UpdateLocalMap();
      mbUpdateLocalMap = false;
    }

    CHECK_RESET;
    // Should we run local bundle adjustment?
    if(!mbBundleConverged_Recent && QueueSize() == 0 && mpMostRecentKF)
    {
      //     std::cout << "13" << std::endl;
      BundleAdjustRecent(mMap, false, false);//TODO: need to set to false (otherwise coordinate system alignment can be a problem?)
    }
    CHECK_RESET;
    // Are there any newly-made map points which need more measurements from older key-frames?
    if(mbBundleConverged_Recent && QueueSize() == 0)
    {
      //     std::cout << "14" << std::endl;
      ReFindNewlyMade();  
    }
    CHECK_RESET;
    // Run global bundle adjustment?
    if(mbBundleConverged_Recent && !mbBundleConverged_Full && QueueSize() == 0)
    {
      //     std::cout << "3" << std::endl;
      BundleAdjust(mMap, mMap->msLocalOptimKeyFrames, mMap->msLocalFixedKeyFrames, mMap->msLocalMapPoints, false, mnGlobalSO3, mnGlobalSE3);
      // If we attempt a global bundle adjustment and it does not converge, use exponential back-off to reduce the size of the map
      // Note bundle adjustment will abort when new keyframe is added, so this controls the time-budget.  Better to get a smaller area to converge
      // than never solve a larger problem because we cannot keep up with the rate of new keyframes.
      if(!mbBundleConverged_Full)
      {
        //       std::cout << "4" << std::endl;
        mnBundleNotConvergedCount++;
        if(mnBundleNotConvergedCount == 15)
        {
          ////    std::cout << "5" << std::endl;
          mnTargetLocalKeyFrames = max(mnTargetLocalKeyFrames-1, mnMinLocalKeyFrames);
          mnBundleNotConvergedCount = 0;
        }
      }
      else
      {
        //      std::cout << "6" << std::endl;
        mnBundleNotConvergedCount = 0;
        if(mMap->msLocalOptimKeyFrames.size() == mnTargetLocalKeyFrames) // Only increment map size if limit was actually reached
          mnTargetLocalKeyFrames++;
      }

      //std::cout << "Target KF: " << mnTargetLocalKeyFrames << " " << mbBundleConverged_Full << " " << mnBundleNotConvergedCount << std::endl;
    }

    /*CHECK_RESET;
    // Are there any old map sections which should be merged with the current map?
    if(mbBundleConverged_Full && !mvOldMaps.empty() && mMap->mrtAllKeyFrames.Size() > mnMinMergeSize)
      MergeMaps();  */

    CHECK_RESET;
    // Very low priorty: re-find measurements marked as outliers
    if(mbBundleConverged_Recent && mbBundleConverged_Full && rand()%20 == 0 && QueueSize() == 0)
    {
      //  std::cout << "16" << std::endl;
      ReFindFromFailureQueue();
    }
    CHECK_RESET;
    HandleBadPoints();
    
    CHECK_RESET;
    // Any new key-frames to be added?
    if(QueueSize() > 0)
    {
      //   std::cout << "22" << std::endl;
      AddKeyFrameFromTopOfQueue(); // Integrate into map data struct, and process
      //ComputeMesh();
    }
  }
}


// Tracker calls this to demand a reset
void MapMaker::RequestReset(bool bCompleteReset)
{
  mbResetDone = false;
  mbResetRequested = true;
  if(bCompleteReset)
    mResetType = COMPLETE;
  else
    mResetType = NEW_MAP;
}

/*void MapMaker::MergeMaps()
{
  if(!mvOldMaps.empty())
  {
    Map *mMergeMap = mvOldMaps.back();
    mvOldMaps.pop_back();
    mMap->Merge(mMergeMap);
    delete mMergeMap;
    std::cout << "Merged old map" << std::endl;
  }
}*/

bool MapMaker::ResetDone()
{
  return mbResetDone;
}

void MapMaker::UpdateLocalMap()
{
  boost::unique_lock< boost::shared_mutex > lockMapPoints(mMap->mMapPointsLock);
  boost::unique_lock< boost::shared_mutex > lockKeyFrames(mMap->mKeyFramesLock);

  mMap->msLocalOptimKeyFrames.clear();
  mMap->msLocalFixedKeyFrames.clear();
  mMap->msLocalMapPoints.clear();

  // Insert the nearest keyframe and all its mappoints into the optimised sets
  if(mpNearestTrackerKF->bFixed)
    mMap->msLocalFixedKeyFrames.insert(mpNearestTrackerKF);
  else
    mMap->msLocalOptimKeyFrames.insert(mpNearestTrackerKF);

  map<boost::shared_ptr<MapPoint>,Measurement> &mKFMeas = mpNearestTrackerKF->mMeasurements;
  for(std::map<boost::shared_ptr<MapPoint>, Measurement>::iterator jiter = mKFMeas.begin(); jiter!= mKFMeas.end(); jiter++)
    if(!jiter->first->bBad)
      mMap->msLocalMapPoints.insert(jiter->first);

  // Fetch the maximum number of additional keyframes we can handle
  std::vector<boost::shared_ptr<KeyFrame> > vKeyframesClosest = RTreeNClosestKeyFrames(mpNearestTrackerKF, mnTargetLocalKeyFrames-1);

  // Keep adding keyframes until they are all added, or the maximum number of mappoints is reached
  for(vector<boost::shared_ptr<KeyFrame> >::iterator iter = vKeyframesClosest.begin(); iter != vKeyframesClosest.end(); iter++)
  {
    if((*iter)->bFixed)
      mMap->msLocalFixedKeyFrames.insert(*iter);
    else
      mMap->msLocalOptimKeyFrames.insert(*iter);

    map<boost::shared_ptr<MapPoint>,Measurement> &mKFMeas = (*iter)->mMeasurements;
    for(std::map<boost::shared_ptr<MapPoint>, Measurement>::iterator jiter = mKFMeas.begin(); jiter!= mKFMeas.end(); jiter++)
      if(!jiter->first->bBad)
        mMap->msLocalMapPoints.insert(jiter->first);

    if(mMap->msLocalMapPoints.size() >= mnTargetLocalPoints)
      break;
  }

  // Now construct the set of all keyframes which observe the local mappoints
  std::set<boost::shared_ptr<KeyFrame> > sAllKeyFrames;
  for(set<boost::shared_ptr<MapPoint> >::iterator it = mMap->msLocalMapPoints.begin(); it!=mMap->msLocalMapPoints.end(); it++)
    sAllKeyFrames.insert((*it)->sMeasurementKFs.begin(), (*it)->sMeasurementKFs.end());

  // Now determine the difference between the set of all keyframes and the set of optimised keyframes to identify the set of fixed keyframes...
  std::set_difference(sAllKeyFrames.begin(), sAllKeyFrames.end(), mMap->msLocalOptimKeyFrames.begin(), mMap->msLocalOptimKeyFrames.end(), std::inserter(mMap->msLocalFixedKeyFrames, mMap->msLocalFixedKeyFrames.begin()));

  lockMapPoints.unlock();
  lockKeyFrames.unlock();
}

// HandleBadPoints() Does some heuristic checks on all points in the map to see if 
// they should be flagged as bad, based on tracker feedback.
void MapMaker::HandleBadPoints()
{
  std::set<boost::shared_ptr<MapPoint> > sBadMapPoints;

  // Did the tracker see any points as an outlier more often than as an inlier?
  for(std::set<boost::shared_ptr<MapPoint> >::iterator it = mMap->msLocalMapPoints.begin(); it != mMap->msLocalMapPoints.end(); ++it)
    if(!(*it)->bFixed)
      if(((*it)->nMEstimatorOutlierCount > 20 && (*it)->nMEstimatorOutlierCount > (*it)->nMEstimatorInlierCount) || (*it)->bBad) //Check for already marked?
        sBadMapPoints.insert(*it);

  // Did the bundle adjuster struggle with any keyframes?
  boost::unique_lock< boost::shared_mutex > lockKeyFrames(mMap->mKeyFramesLock);
  /*for(std::set<boost::shared_ptr<KeyFrame> >::iterator it = mMap->msLocalOptimKeyFrames.begin(); it != mMap->msLocalOptimKeyFrames.end();)
  {
    boost::shared_ptr<KeyFrame> pk = (*it);

    if(pk->nOutlierCount >= 10)
    {
      std::cout << "Deleting bad keyframe" << std::endl;
      for(std::map<boost::shared_ptr<MapPoint>, Measurement>::iterator jt = pk->mMeasurements.begin(); jt != pk->mMeasurements.end(); jt++)
      {
        jt->first->sMeasurementKFs.erase(pk); // Erase all measurements tied to this keyframe
        if(jt->first->pPatchSourceKF == pk) // Check this is not the mappoints source keyframe, if so it is likely a bad mappoint
          sBadMapPoints.insert(jt->first);
      }

      // Should be no remaining record of this keyframe so delete it!
      Vector<3> v3Pos = pk->se3CfromW.inverse().get_translation();
      mMap->mrtAllKeyFrames.Remove(v3Pos.get_data_ptr(), pk); // delete it from the tree
      delete pk; // delete the actual keyframe
      std::set<boost::shared_ptr<KeyFrame> >::iterator itdelete = it;
      ++it;   // increment before erasing!
      mMap->msLocalOptimKeyFrames.erase(itdelete); // delete the local map copy
    }
    else
      ++it;
  }*/

  // All points marked as bad will be erased - erase all records of them
  // from keyframes in which they might have been measured.
  boost::unique_lock< boost::shared_mutex > lockMapPoints(mMap->mMapPointsLock);
  std::set<boost::shared_ptr<KeyFrame> > sModifiedKeyFrames;
  for(std::set<boost::shared_ptr<MapPoint> >::iterator it = sBadMapPoints.begin(); it != sBadMapPoints.end(); ++it)
  {
    // Erase all measurements of this mappoint from keyframes
    for(std::set<boost::shared_ptr<KeyFrame> >::iterator jt = (*it)->sMeasurementKFs.begin(); jt != (*it)->sMeasurementKFs.end(); ++jt)
    {
      (*jt)->mMeasurements.erase(*it); 
      sModifiedKeyFrames.insert(*jt);
    }

    // Erase the mappoint from the local map
    if(mMap->msLocalMapPoints.count(*it))
      mMap->msLocalMapPoints.erase(*it);

    // Ensure the mappoint is marked as bad (will flush out any remaining references)
    (*it)->bBad = true;
  }

  // Refresh the scene depth for keyframes which had mappoints removed
  for(std::set<boost::shared_ptr<KeyFrame> >::iterator it = sModifiedKeyFrames.begin(); it != sModifiedKeyFrames.end(); ++it)
    RefreshSceneDepth(*it);

  // Remove bad mappoints from the global map -> if no references remain they are destroyed...
  std::vector<boost::shared_ptr<MapPoint> >::iterator new_end = partition(mMap->mvAllMapPoints.begin(), mMap->mvAllMapPoints.end(), IsMapPointGood);
  mMap->mvAllMapPoints.erase(new_end, mMap->mvAllMapPoints.end());

  lockKeyFrames.unlock();
  lockMapPoints.unlock();
}

MapMaker::~MapMaker()
{
  mbBundleAbortRequested = true;
  stop(); // makes shouldStop() return true
  cout << "Waiting for mapmaker to die.." << endl;
  join();
  if(ofsLogger.is_open())
    ofsLogger.close();
  cout << " .. mapmaker has died." << endl;
}


// Finds 3d coords of point in reference frame B from two z=1 plane projections
Vector<3> MapMaker::ReprojectPoint(SE3<> se3AfromB, const Vector<2> &v2A, const Vector<2> &v2B)
{
  Matrix<3,4> PDash;
  PDash.slice<0,0,3,3>() = se3AfromB.get_rotation().get_matrix();
  PDash.slice<0,3,3,1>() = se3AfromB.get_translation().as_col();
  
  Matrix<4> A;
  A[0][0] = -1.0; A[0][1] =  0.0; A[0][2] = v2B[0]; A[0][3] = 0.0;
  A[1][0] =  0.0; A[1][1] = -1.0; A[1][2] = v2B[1]; A[1][3] = 0.0;
  A[2] = v2A[0] * PDash[2] - PDash[0];
  A[3] = v2A[1] * PDash[2] - PDash[1];

  SVD<4,4> svd(A);
  Vector<4> v4Smallest = svd.get_VT()[3];
  if(v4Smallest[3] == 0.0)
    v4Smallest[3] = 0.00001;
  return project(v4Smallest);
}

bool MapMaker::InitFromKeyFrames(std::vector<boost::shared_ptr<KeyFrame> > &vKeyFrames, std::map<int, MarkerMeasurements> &mMarkerMeasurements, SE3<> &se3Pose)
{
  int nFirst, nMiddle, nLast;

  std::cout << "Baseline: " << MaxBaseline(vKeyFrames, nFirst, nMiddle, nLast) << std::endl;

  if(vKeyFrames.size() < 4 || MaxBaseline(vKeyFrames, nFirst, nMiddle, nLast) < mdKeyFrameSpacing)
  {
    cout << "Insufficient number of frames or poor baseline for successful map initialisation." << endl;
    vKeyFrames.clear();
    mResetType = PREV_MAP;
    Reset();
    return false;
  }

  boost::shared_ptr<KeyFrame> kF = vKeyFrames[nFirst];
  boost::shared_ptr<KeyFrame> kM = vKeyFrames[nMiddle];
  boost::shared_ptr<KeyFrame> kL = vKeyFrames[nLast];

  // If we are using external absolute pose measurements, an acceptable quality pose estimate must be available for the first frame
  // in order to work out the transformation between the coordinate systems (could do this later, but makes life simpler)
  if((mnGlobalSO3 || mnGlobalSE3) && !kF->mse3PdfGfromC.has_rotation())
  {
    cout << "No external attitude measurement available for first keyframe, try again." << endl;
    vKeyFrames.clear();
    mResetType = PREV_MAP;
    Reset();
    return false;
  }

  if(mnGlobalSE3 && !kF->mse3PdfGfromC.has_translation())
  {
    cout << "No external position measurements available for first keyframe, try again." << endl;
    vKeyFrames.clear();
    mResetType = PREV_MAP;
    Reset();
    return false;
  }

  kF->bFixed = true;
  kM->bFixed = false;
  kL->bFixed = false;

  kF->MakeKeyFrame_Rest(mScarf);
  kM->MakeKeyFrame_Rest(mScarf);
  kL->MakeKeyFrame_Rest(mScarf);

  boost::unique_lock< boost::shared_mutex > lockKeyFrames(mMap->mKeyFramesLock);
  if(kF->bFixed)
    mMap->msLocalFixedKeyFrames.insert(kF);
  else
    mMap->msLocalOptimKeyFrames.insert(kF);
  Vector<3> v3kF_CamPos = kF->se3CfromW.inverse().get_translation();
  mMap->mrtAllKeyFrames.Insert(v3kF_CamPos.get_data_ptr(), kF);

  mMap->msLocalOptimKeyFrames.insert(kM);
  Vector<3> v3kM_CamPos = kM->se3CfromW.inverse().get_translation();
  mMap->mrtAllKeyFrames.Insert(v3kM_CamPos.get_data_ptr(), kM);

  mMap->msLocalOptimKeyFrames.insert(kL);
  Vector<3> v3kL_CamPos = kL->se3CfromW.inverse().get_translation();
  mMap->mrtAllKeyFrames.Insert(v3kL_CamPos.get_data_ptr(), kL);
  lockKeyFrames.unlock();

  // Construct map from the stereo matches.
  PatchFinder finder;

  int count = 0;
  for(std::map<int, MarkerMeasurements>::iterator it = mMarkerMeasurements.begin(); it != mMarkerMeasurements.end(); ++it)
  {
    MarkerMeasurements& mkr = it->second;
    std::vector<boost::shared_ptr<KeyFrame> > vMeasurementKF;
    if(mkr.mMeasurements.count(kF))
      vMeasurementKF.push_back(kF);
    if(mkr.mMeasurements.count(kM))
      vMeasurementKF.push_back(kM);
    if(mkr.mMeasurements.count(kL))
      vMeasurementKF.push_back(kL);

    if(vMeasurementKF.size() >= 2) // Cannot add a point if there is only one observation
    {
      boost::shared_ptr<MapPoint> p(new MapPoint());
      count++;
      // Patch source stuff:
      p->pPatchSourceKF = vMeasurementKF[0];
      p->nSourceLevel = 0;
      p->irCenter = mkr.mMeasurements[vMeasurementKF[0]];

      //Steal a descriptor from a closest candidate...
      p->pDescriptor = NULL;
      unsigned int nMinMagSquared = 10*10;
      for(unsigned int l = 0; l < LEVELS; ++l)
      {
        vector<Candidate> &vLevelCandidates = vMeasurementKF[0]->aLevels[l].vCandidates;
        for(unsigned int c = 0; c < vLevelCandidates.size(); ++c)
        {
          unsigned int nMagSquared = (p->irCenter - vLevelCandidates[c].irLevelPos).mag_squared();
      	  if(nMagSquared < nMinMagSquared)
          {
            nMinMagSquared = nMagSquared;
            p->pDescriptor = &(vLevelCandidates[c].descriptor[0]);
          }
        } 
      }

      if(p->pDescriptor == NULL)
        continue;

      p->v3Normal_NC = makeVector( 0,0,-1);
      p->v3Center_NC = unit(unproject(mCamera.UnProject(p->irCenter)));
      p->v3OneDownFromCenter_NC = unit(unproject(mCamera.UnProject(p->irCenter + ImageRef(0,1))));
      p->v3OneRightFromCenter_NC = unit(unproject(mCamera.UnProject(p->irCenter + ImageRef(1,0))));
      p->RefreshPixelVectors();
      p->v3WorldPos = mkr.v3WorldPos;
      p->bFixed = true;

      boost::unique_lock< boost::shared_mutex > lockMapPoints(mMap->mMapPointsLock);
      mMap->msLocalMapPoints.insert(p);
      mMap->mvAllMapPoints.push_back(p);
      lockMapPoints.unlock();  

      finder.MakeTemplateCoarseNoWarp(p);
      finder.MakeSubPixTemplate();
      for(int i = 1; i < vMeasurementKF.size(); ++i)
      {
        finder.SetSubPixPos(vec(mkr.mMeasurements[vMeasurementKF[i]]));

        bool bGood = finder.IterateSubPixToConvergence(*vMeasurementKF[i], 10);
        if(!bGood)
          continue;
    
        Measurement mMeas;
        mMeas.nLevel = 0;
        mMeas.Source = Measurement::SRC_TRAIL;
        mMeas.v2RootPos = finder.GetSubPixPos();
        mMeas.bSubPix = true;
        vMeasurementKF[i]->mMeasurements[p] = mMeas;
        p->sMeasurementKFs.insert(vMeasurementKF[i]);
      }
    }
  }

  std::cout << "Marker points: " << count << std::endl;

  for(int nLevel = 0; nLevel < 4; ++nLevel)
  {
    for(unsigned int i = 0; i < kL->aLevels[nLevel].vCandidates.size(); i++)
      AddPointEpipolar(kL, kF, nLevel, i);

    ThinCandidates(kM, nLevel);

    for(unsigned int i = 0; i < kL->aLevels[nLevel].vCandidates.size(); i++)
    {
      AddPointEpipolar(kM, kF, nLevel, i);
      AddPointEpipolar(kM, kL, nLevel, i);
    }
  }

  for(int i=0; i<5; i++)
  {
    BundleAdjust(mMap, mMap->msLocalOptimKeyFrames, mMap->msLocalFixedKeyFrames, mMap->msLocalMapPoints, false, false, false);
    if(mbResetRequested)
    {
      cout << " Error occurred in initial bundle adjustment, try again." << endl;
      vKeyFrames.clear();
      mResetType = PREV_MAP;
      Reset();
      return false;
    }
  }

  //First frame handled differently as relative motion is Identity/Zeros
  mMap->mpFirstKeyFrame = kF;
  kF->mse3PdfFfromC = SE3GaussianPDF(SO3<>(), Zeros, kF->mse3PdfGfromC.get_covariance());

  kM->ComputeGlobalFfromC(mMap->GetGfromF());
  kL->ComputeGlobalFfromC(mMap->GetGfromF());

  // Estimate the feature depth distribution in the first two key-frames
  // (Needed for epipolar search)
  if (!RefreshSceneDepth(kF) || !RefreshSceneDepth(kM) || !RefreshSceneDepth(kL)){
    std::cout << "Refreshing Scene Depth: Something is wrong with the initial KFs" << std::endl;
    vKeyFrames.clear();
    mResetType = PREV_MAP;
    Reset();
    return false;
  }

  int nAddedMapPoints = 0;
  nAddedMapPoints += AddSomeMapPoints(kL, 0);
  nAddedMapPoints += AddSomeMapPoints(kL, 3);
  nAddedMapPoints += AddSomeMapPoints(kL, 1);
  nAddedMapPoints += AddSomeMapPoints(kL, 2);

  if(nAddedMapPoints == 0)
  {
    cout << "Adding Map Points: Something is wrong with the initial KFs, try again." << endl;
    vKeyFrames.clear();
    mResetType = PREV_MAP;
    Reset();
    return false;
  }

  mbBundleConverged_Full = false;
  mbBundleConverged_Recent = false;
  
  unsigned int iterations = 0;
  while(!mbBundleConverged_Full)
  {
    BundleAdjust(mMap, mMap->msLocalOptimKeyFrames, mMap->msLocalFixedKeyFrames, mMap->msLocalMapPoints, false, mnGlobalSO3, mnGlobalSE3);
    if(mbResetRequested)
    {
      vKeyFrames.clear();
      mResetType = PREV_MAP;
      Reset();
      return false;
    }
    else if(++iterations == 10)
    {
      cout << "Error occurred in initial bundle adjustment, try again." << endl;
      vKeyFrames.clear();
      mResetType = PREV_MAP;
      Reset();
      return false;
    }
  }

  // Estimate the feature depth distribution in the first two key-frames
  // (Needed for epipolar search)
  if (!RefreshSceneDepth(kF) || !RefreshSceneDepth(kM) || !RefreshSceneDepth(kL)){
    std::cout << "Refreshing Scene Depth: Something is wrong with the initial KFs" << std::endl;
    vKeyFrames.clear();
    mResetType = PREV_MAP;
    Reset();
    return false;
  }

  kF->bFixed = false; //TODO: should remain true? -> unfix to enable relative transform between world and marker to be updated
  mMap->SetGood();
  se3Pose = kL->se3CfromW;
  mpMostRecentKF = kL;
  mpNearestTrackerKF = kL;

  vKeyFrames.clear();

  return true; 
}

double MapMaker::MaxBaseline(std::vector<boost::shared_ptr<KeyFrame> > &vKeyFrames, int &nFirst, int &nMiddle, int &nLast)
{
  // Find maximum baseline
  double dMaxBaseline = 0.0;
  for(int i = 0; i < vKeyFrames.size(); ++i)
  {
    for(int j = i+1; j < vKeyFrames.size(); ++j)
    {
      Vector<3> v3Diff = vKeyFrames[i]->se3CfromW.inverse().get_translation() - vKeyFrames[j]->se3CfromW.inverse().get_translation();
      double dBaseline = sqrt(v3Diff[0]*v3Diff[0] + v3Diff[1]*v3Diff[1]);
      if(dBaseline > dMaxBaseline)
      {
        nFirst = i;
        nLast = j;
        dMaxBaseline = dBaseline;
      }
    }
  }

  // Find middle frame which is furthest from the two frames with greatest baseline
  double dMaxMiddle  = 0.0;
  for(int i = 0; i < vKeyFrames.size(); ++i)
  {
    if(i != nFirst && i != nLast)
    {
      Vector<3> v3DiffLast = vKeyFrames[i]->se3CfromW.inverse().get_translation() - vKeyFrames[nLast]->se3CfromW.inverse().get_translation();
      double dBaselineLast = sqrt(v3DiffLast[0]*v3DiffLast[0] + v3DiffLast[1]*v3DiffLast[1]);
      Vector<3> v3DiffFirst = vKeyFrames[i]->se3CfromW.inverse().get_translation() - vKeyFrames[nFirst]->se3CfromW.inverse().get_translation();
      double dBaselineFirst = sqrt(v3DiffFirst[0]*v3DiffFirst[0] + v3DiffFirst[1]*v3DiffFirst[1]);
      double dBaseline = min(dBaselineFirst, dBaselineLast);

      if(dBaseline > dMaxMiddle)
      {
        nMiddle = i;
        dMaxMiddle = dBaseline;
      }
    }
  }

  return dMaxBaseline;
}

// ThinCandidates() Thins out a key-frame's candidate list.
// Candidates are those salient corners where the mapmaker will attempt 
// to make a new map point by epipolar search. We don't want to make new points
// where there are already existing map points, this routine erases such candidates.
// Operates on a single level of a keyframe.
void MapMaker::ThinCandidates(boost::shared_ptr<KeyFrame> k, int nLevel)
{
  vector<Candidate> &vCSrc = k->aLevels[nLevel].vCandidates;
  vector<Candidate> vCGood;
  vector<ImageRef> irBusyLevelPos;
  // Make a list of `busy' image locations, which already have features at the same level
  // or at one level higher.
  for(std::map<boost::shared_ptr<MapPoint>, Measurement>::iterator it = k->mMeasurements.begin(); it!=k->mMeasurements.end(); it++)
  {
    if(!(it->second.nLevel == nLevel || it->second.nLevel == nLevel + 1))
      continue;
    irBusyLevelPos.push_back(ir_rounded(it->second.v2RootPos / LevelScale(nLevel)));
  }
  
  // Only keep those candidates further than 10 pixels away from busy positions.
  unsigned int nMinMagSquared = 10*10;
  for(unsigned int i=0; i<vCSrc.size(); i++)
  {
    ImageRef irC = vCSrc[i].irLevelPos;
    bool bGood = true;
    for(unsigned int j=0; j<irBusyLevelPos.size(); j++)
  	{
  	  ImageRef irB = irBusyLevelPos[j];
  	  if((irB - irC).mag_squared() < nMinMagSquared) //TODO: add check to prefer candidates with SCARF descriptor?
	    {
	      bGood = false;
	      break;
	    }
  	}

    if(bGood)
    	vCGood.push_back(vCSrc[i]);
  } 
  vCSrc = vCGood;
}

// Adds map points by epipolar search to the last-added key-frame, at a single
// specified pyramid level. Does epipolar search in the target keyframe as closest by
// the ClosestKeyFrame function.
int MapMaker::AddSomeMapPoints(boost::shared_ptr<KeyFrame> k, int nLevel)
{
  int nMapPointsAdded = 0;
  boost::shared_ptr<KeyFrame> pClosestKF = RTreeClosestKeyFrame(k);   

  ThinCandidates(k, nLevel);

  for(unsigned int i = 0; i < k->aLevels[nLevel].vCandidates.size(); i++)
    if(AddPointEpipolar(k, pClosestKF, nLevel, i))
      nMapPointsAdded++;

  return nMapPointsAdded;
};


// The tracker entry point for adding a new keyframe;
// the tracker thread doesn't want to hang about, so 
// just dumps it on the top of the mapmaker's queue to 
// be dealt with later, and return.
void MapMaker::AddKeyFrame(boost::shared_ptr<KeyFrame> pK)
{
  mvpKeyFrameQueue.push_back(pK);
  if(mbBundleRunning)   // Tell the mapmaker to stop doing low-priority stuff and concentrate on this KF first.
    mbBundleAbortRequested = true;
}

void MapMaker::UpdateTrackerPosition(SE3<> se3CfromW)
{
  boost::shared_ptr<KeyFrame> pOldKF = mpNearestTrackerKF;
  mpNearestTrackerKF = RTreeClosestKeyFrame(se3CfromW); // Inform the MapMaker it should be working on this area of the map
  if(mpNearestTrackerKF != pOldKF)
    mbUpdateLocalMap = true; // Eventually will cause the MapMaker to update the local mappoints/keyframes to reflect the new information
}

void MapMaker::ForceLocalMapUpdate(SE3<> se3CfromW)
{
  mpNearestTrackerKF = RTreeClosestKeyFrame(se3CfromW); // Inform the MapMaker it should be working on this area of the map
  mbUpdateLocalMap = true; // Trigger map update, cannot just do it as bundler may be using map
  if(mbBundleRunning)   // Tell the mapmaker to stop doing low-priority stuff and concentrate on this KF first.
    mbBundleAbortRequested = true;

  while(mbUpdateLocalMap) { usleep(100); }
}

// Mapmaker's code to handle incoming key-frames.
void MapMaker::AddKeyFrameFromTopOfQueue()
{
  if(mvpKeyFrameQueue.size() == 0)
    return;

  mvpKeyFrameQueue[0]->MakeKeyFrame_Rest(mScarf);
  mpMostRecentKF = mvpKeyFrameQueue[0];
  mvpKeyFrameQueue.erase(mvpKeyFrameQueue.begin());

  boost::unique_lock< boost::shared_mutex > lockKeyFrames(mMap->mKeyFramesLock);
  mMap->msLocalOptimKeyFrames.insert(mpMostRecentKF);
  Vector<3> v3KF_CamPos = mpMostRecentKF->se3CfromW.inverse().get_translation();
  mMap->mrtAllKeyFrames.Insert(v3KF_CamPos.get_data_ptr(), mpMostRecentKF);
  lockKeyFrames.unlock();

  // Any measurements? Update the relevant point's measurement counter status map
  for(std::map<boost::shared_ptr<MapPoint>, Measurement>::iterator it = mpMostRecentKF->mMeasurements.begin(); it!=mpMostRecentKF->mMeasurements.end(); it++)
  {
    it->first->sMeasurementKFs.insert(mpMostRecentKF);
    it->second.Source = Measurement::SRC_TRACKER;

    /*// Normal of point is approximated by averaging optical axis directions of keyframes in which point is visible
    it->first->v3Normal2 = makeVector(0, 0, 0);
    unsigned int nCount = 0;
    for(std::set<boost::shared_ptr<KeyFrame> >::iterator it2 = it->first->sMeasurementKFs.begin(); it2 != it->first->sMeasurementKFs.end(); ++it2)
    {
      it->first->v3Normal2 += unit((*it2)->se3CfromW.inverse().get_translation() - it->first->v3WorldPos);
      nCount++;
    }
    it->first->v3Normal2 /= nCount;*/
  }
  
  // And maybe we missed some - this now adds to the map itself, too.
  ReFindInSingleKeyFrame(mpMostRecentKF);
  
  AddSomeMapPoints(mpMostRecentKF, 3);       // .. and add more map points by epipolar search.
  AddSomeMapPoints(mpMostRecentKF, 0);
  AddSomeMapPoints(mpMostRecentKF, 1);
  AddSomeMapPoints(mpMostRecentKF, 2);

  mbBundleConverged_Full = false;
  mbBundleConverged_Recent = false;
}

// Tries to make a new map point out of a single candidate point
// by searching for that point in another keyframe, and triangulating
// if a match is found.
bool MapMaker::AddPointEpipolar(boost::shared_ptr<KeyFrame> kSrc, boost::shared_ptr<KeyFrame> kTarget, int nLevel, int nCandidate)
{
  static Image<Vector<2> > imUnProj;
  static bool bMadeCache = false;
  if(!bMadeCache)
  {
    imUnProj.resize(kSrc->aLevels[0].im.size());
    ImageRef ir;
    do imUnProj[ir] = mCamera.UnProject(ir);
    while(ir.next(imUnProj.size()));
    bMadeCache = true;
  }
  
  int nLevelScale = LevelScale(nLevel);
  Candidate &candidate = kSrc->aLevels[nLevel].vCandidates[nCandidate];
  ImageRef irLevelPos = candidate.irLevelPos;
  Vector<2> v2RootPos = LevelZeroPos(irLevelPos, nLevel);
  
  Vector<3> v3Ray_SC = unit(unproject(mCamera.UnProject(v2RootPos)));
  Vector<3> v3LineDirn_TC = kTarget->se3CfromW.get_rotation() * (kSrc->se3CfromW.get_rotation().inverse() * v3Ray_SC);

  // Restrict epipolar search to a relatively narrow depth range to increase reliability
  double dMean = kSrc->dSceneDepthMean;
  double dSigma = kSrc->dSceneDepthSigma;
  double dStartDepth = max(mdKeyFrameSpacing, dMean - dSigma);
  double dEndDepth = min(40 * mdKeyFrameSpacing, dMean + dSigma);
  
  Vector<3> v3CamCenter_TC = kTarget->se3CfromW * kSrc->se3CfromW.inverse().get_translation(); // The camera end
  Vector<3> v3RayStart_TC = v3CamCenter_TC + dStartDepth * v3LineDirn_TC;                               // the far-away end
  Vector<3> v3RayEnd_TC = v3CamCenter_TC + dEndDepth * v3LineDirn_TC;                               // the far-away end

  
  if(v3RayEnd_TC[2] <= v3RayStart_TC[2])  // it's highly unlikely that we'll manage to get anything out if we're facing backwards wrt the other camera's view-ray
    return false;
  if(v3RayEnd_TC[2] <= 0.0 )  
    return false;
  if(v3RayStart_TC[2] <= 0.0)
    v3RayStart_TC += v3LineDirn_TC * (0.001 - v3RayStart_TC[2] / v3LineDirn_TC[2]);
  
  Vector<2> v2A = project(v3RayStart_TC);
  Vector<2> v2B = project(v3RayEnd_TC);
  Vector<2> v2AlongProjectedLine = v2A-v2B;
  
  if(v2AlongProjectedLine * v2AlongProjectedLine < 0.00000001)
  {
    //cout << "v2AlongProjectedLine too small." << endl;
    return false;
  }
  normalize(v2AlongProjectedLine);
  Vector<2> v2Normal;
  v2Normal[0] = v2AlongProjectedLine[1];
  v2Normal[1] = -v2AlongProjectedLine[0];
  
  double dNormDist = v2A * v2Normal;
  if(fabs(dNormDist) > mCamera.LargestRadiusInImage())
    return false;
  
  double dMinLen = min(v2AlongProjectedLine * v2A, v2AlongProjectedLine * v2B) - 0.05;
  double dMaxLen = max(v2AlongProjectedLine * v2A, v2AlongProjectedLine * v2B) + 0.05;
  if(dMinLen < -2.0)  dMinLen = -2.0;
  if(dMaxLen < -2.0)  dMaxLen = -2.0;
  if(dMinLen > 2.0)   dMinLen = 2.0;
  if(dMaxLen > 2.0)   dMaxLen = 2.0;

  // Find current-frame corners which might match this
  PatchFinder Finder;
  Finder.MakeTemplateCoarseNoWarp(*kSrc, nLevel, irLevelPos);
  if(Finder.TemplateBad())  return false;
  
  vector<Vector<2> > &vv2Corners = kTarget->aLevels[nLevel].vImplaneCorners;
  vector<ImageRef> &vIR = kTarget->aLevels[nLevel].vCorners;
  if(!kTarget->aLevels[nLevel].bImplaneCornersCached)
    {
      for(unsigned int i=0; i<vIR.size(); i++)   // over all corners in target img..
	vv2Corners.push_back(imUnProj[ir(LevelZeroPos(vIR[i], nLevel))]);
      kTarget->aLevels[nLevel].bImplaneCornersCached = true;
    }
  
  int nBest = -1;
  int nBestZMSSD = Finder.mnMaxSSD + 1;
  double dMaxDistDiff = mCamera.OnePixelDist() * (4.0 + 1.0 * nLevelScale);
  double dMaxDistSq = dMaxDistDiff * dMaxDistDiff;
  
  for(unsigned int i=0; i<vv2Corners.size(); i++)   // over all corners in target img..
    {
      Vector<2> v2Im = vv2Corners[i];
      double dDistDiff = dNormDist - v2Im * v2Normal;
      if(dDistDiff * dDistDiff > dMaxDistSq)	continue; // skip if not along epi line
      if(v2Im * v2AlongProjectedLine < dMinLen)	continue; // skip if not far enough along line
      if(v2Im * v2AlongProjectedLine > dMaxLen)	continue; // or too far
      int nZMSSD = Finder.ZMSSDAtPoint(kTarget->aLevels[nLevel].im, vIR[i]);
      if(nZMSSD < nBestZMSSD)
	{
	  nBest = i;
	  nBestZMSSD = nZMSSD;
	}
    } 
  
  if(nBest == -1)   return false;   // Nothing found.
  
  //  Found a likely candidate along epipolar ray
  Finder.MakeSubPixTemplate();
  Finder.SetSubPixPos(LevelZeroPos(vIR[nBest], nLevel));
  bool bSubPixConverges = Finder.IterateSubPixToConvergence(*kTarget,10);
  if(!bSubPixConverges)
    return false;
  
  // Now triangulate the 3d point...
  Vector<3> v3Cam = ReprojectPoint(kSrc->se3CfromW * kTarget->se3CfromW.inverse(),
		   mCamera.UnProject(v2RootPos), 
		   mCamera.UnProject(Finder.GetSubPixPos()));

  //if(v3Cam[2] > (dMean + dSigma)) Cannot do this!
//    return false;
  
  boost::shared_ptr<MapPoint> pNew(new MapPoint);
  pNew->v3WorldPos = kTarget->se3CfromW.inverse() * v3Cam;
  //pp->v3Normal2 = makeVector(0,0,-1);
  
  // Patch source stuff:
  pNew->pPatchSourceKF = kSrc;
  pNew->nSourceLevel = nLevel;
  pNew->pDescriptor = candidate.descriptor;
  pNew->v3Normal_NC = makeVector( 0,0,-1);
  pNew->irCenter = irLevelPos;
  pNew->v3Center_NC = unit(unproject(mCamera.UnProject(v2RootPos)));
  pNew->v3OneRightFromCenter_NC = unit(unproject(mCamera.UnProject(v2RootPos + vec(ImageRef(nLevelScale,0)))));
  pNew->v3OneDownFromCenter_NC  = unit(unproject(mCamera.UnProject(v2RootPos + vec(ImageRef(0,nLevelScale)))));
  pNew->RefreshPixelVectors();

  boost::unique_lock< boost::shared_mutex > lockMapPoints(mMap->mMapPointsLock);
  mMap->msLocalMapPoints.insert(pNew);
  mMap->mvAllMapPoints.push_back(pNew);
  lockMapPoints.unlock();

  mvNewQueue.push_back(pNew);
  Measurement m;
  m.Source = Measurement::SRC_ROOT;
  m.v2RootPos = v2RootPos;
  m.nLevel = nLevel;
  m.bSubPix = true;
  kSrc->mMeasurements[pNew] = m;

  m.Source = Measurement::SRC_EPIPOLAR;
  m.v2RootPos = Finder.GetSubPixPos();
  kTarget->mMeasurements[pNew] = m;
  pNew->sMeasurementKFs.insert(kSrc);
  pNew->sMeasurementKFs.insert(kTarget);
  return true;
}

double MapMaker::KeyFrameLinearDist(boost::shared_ptr<KeyFrame> k1, boost::shared_ptr<KeyFrame> k2)
{
  Vector<3> v3KF1_CamPos = k1->se3CfromW.inverse().get_translation();
  Vector<3> v3KF2_CamPos = k2->se3CfromW.inverse().get_translation();
  Vector<3> v3Diff = v3KF2_CamPos - v3KF1_CamPos;
  double dDist = sqrt(v3Diff * v3Diff);
  return dDist;
}

vector<boost::shared_ptr<KeyFrame> > MapMaker::RTreeNClosestKeyFrames(boost::shared_ptr<KeyFrame> k, unsigned int N)
{
  Vector<3> v3KF_CamPos = k->se3CfromW.inverse().get_translation();
  std::vector<std::pair<double, boost::shared_ptr<KeyFrame> > > vClosestKeyFrames;

  // Note: Neighbours are sorted by their proximity
  mMap->mrtAllKeyFrames.NearestNeighbours(v3KF_CamPos.get_data_ptr(), k, N, vClosestKeyFrames, 2); //Only consider first two dimensions for distance (x,y)  

  vector<boost::shared_ptr<KeyFrame> > vResult;
  for(unsigned int i=0; i<vClosestKeyFrames.size(); i++)
    vResult.push_back(vClosestKeyFrames[i].second);

  return vResult;  
}

boost::shared_ptr<KeyFrame> MapMaker::RTreeClosestKeyFrame(boost::shared_ptr<KeyFrame> k)
{
  Vector<3,double> v3KF_CamPos = k->se3CfromW.inverse().get_translation();
  std::pair<double, boost::shared_ptr<KeyFrame> > pClosest;

  mMap->mrtAllKeyFrames.NearestNeighbour(v3KF_CamPos.get_data_ptr(), k, pClosest, 3); //Consider all three dimensions for distance  

  return pClosest.second;  
}

boost::shared_ptr<KeyFrame> MapMaker::RTreeClosestKeyFrame(SE3<> se3CfromW)
{
  Vector<3,double> v3CamPos = se3CfromW.inverse().get_translation();
  std::pair<double, boost::shared_ptr<KeyFrame> > pClosest;

  mMap->mrtAllKeyFrames.NearestNeighbour(v3CamPos.get_data_ptr(), boost::shared_ptr<KeyFrame>(), pClosest, 3); //Consider all three dimensions for distance  

  return pClosest.second;  
}

double MapMaker::DistToNearestKeyFrame(boost::shared_ptr<KeyFrame> kCurrent)
{
  boost::shared_ptr<KeyFrame> kClosest = RTreeClosestKeyFrame(kCurrent);
  double dDist = KeyFrameLinearDist(kCurrent, kClosest);
  return dDist;
}

bool MapMaker::NeedNewKeyFrame(boost::shared_ptr<KeyFrame> kCurrent, double dMaxKFDistWiggleMult)
{
  boost::shared_ptr<KeyFrame> kClosest = RTreeClosestKeyFrame(kCurrent);
  double dDist = KeyFrameLinearDist(kCurrent, kClosest);

  if(dDist > dMaxKFDistWiggleMult * mdKeyFrameSpacing)
    return true;
  else
    return false;
}

// Peform a local bundle adjustment which only adjusts
// recently added key-frames
void MapMaker::BundleAdjustRecent(Map *pMap, bool bGlobalSE3, bool bGlobalSO3)
{
  if(pMap->msLocalOptimKeyFrames.size() < 8)  
  { // Ignore this unless map is big enough
    mbBundleConverged_Recent = true;
    return;
  }

  // First, make a list of the keyframes we want adjusted in the adjuster.
  // This will be the last keyframe inserted, and its four nearest neighbors
  set<boost::shared_ptr<KeyFrame> > sAdjustSet;
  sAdjustSet.insert(mpMostRecentKF);
  vector<boost::shared_ptr<KeyFrame> > vClosest = RTreeNClosestKeyFrames(mpMostRecentKF, 4);

  for(int i=0; i<4; i++)
    if(vClosest[i]->bFixed == false)
      sAdjustSet.insert(vClosest[i]);
  
  // Now we find the set of features which they contain.
  set<boost::shared_ptr<MapPoint> > sMapPoints;
  for(set<boost::shared_ptr<KeyFrame> >::iterator iter = sAdjustSet.begin(); iter!=sAdjustSet.end(); iter++)
  {
    map<boost::shared_ptr<MapPoint> ,Measurement> &mKFMeas = (*iter)->mMeasurements;
    for(std::map<boost::shared_ptr<MapPoint>, Measurement>::iterator jiter = mKFMeas.begin(); jiter!= mKFMeas.end(); jiter++)
      sMapPoints.insert(jiter->first);
  };
  
  // Finally, add all keyframes which measure above points as fixed keyframes
  set<boost::shared_ptr<KeyFrame> > sFixedSet; //TODO: best approach???
  for(set<boost::shared_ptr<KeyFrame> >::iterator it = pMap->msLocalOptimKeyFrames.begin(); it!=pMap->msLocalOptimKeyFrames.end(); it++)
  {
    if(sAdjustSet.count(*it))
      continue;

    bool bInclude = false;
    for(std::map<boost::shared_ptr<MapPoint>, Measurement>::iterator jiter = (*it)->mMeasurements.begin(); jiter!= (*it)->mMeasurements.end(); jiter++)
      if(sMapPoints.count(jiter->first))
      {
        bInclude = true;
        break;
      }

    if(bInclude)
      sFixedSet.insert(*it);
  }
  
  BundleAdjust(pMap, sAdjustSet, sFixedSet, sMapPoints, true, bGlobalSE3, bGlobalSO3);
}

// Common bundle adjustment code. This creates a bundle-adjust instance, populates it, and runs it.
void MapMaker::BundleAdjust(Map *pMap, set<boost::shared_ptr<KeyFrame> > sAdjustSet, set<boost::shared_ptr<KeyFrame> > sFixedSet, set<boost::shared_ptr<MapPoint> > sMapPoints, bool bRecent, bool bGlobalSE3, bool bGlobalSO3)
{
  Bundle b(mCamera);   // Our bundle adjuster
  mbBundleRunning = true;
  mbBundleRunningIsRecent = bRecent;

  b.SetFfromW(pMap->GetPtrFfromW());
  
  // The bundle adjuster does different accounting of keyframes and map points;
  // Translation maps are stored:
  map<boost::shared_ptr<MapPoint>, int> mPoint_BundleID;
  map<int, boost::shared_ptr<MapPoint> > mBundleID_Point;
  map<boost::shared_ptr<KeyFrame>, int> mView_BundleID;
  map<int, boost::shared_ptr<KeyFrame> > mBundleID_View;
  
  // Add the keyframes' poses to the bundle adjuster. Two parts: first nonfixed, then fixed.
  for(set<boost::shared_ptr<KeyFrame> >::iterator it = sAdjustSet.begin(); it!= sAdjustSet.end(); it++)
  {
    int nBundleID;
    //std::cout << (*it)->se3CfromW.get_rotation().inverse() << " " << (*it)->mse3GfromC.get_rotation() << std::endl;
    nBundleID = b.AddCamera((*it)->se3CfromW, (*it)->bFixed, (*it)->mse3PdfFfromC);
    mView_BundleID[*it] = nBundleID;
    mBundleID_View[nBundleID] = *it;
  }

  for(set<boost::shared_ptr<KeyFrame> >::iterator it = sFixedSet.begin(); it!= sFixedSet.end(); it++)
  {
    int nBundleID;
    //std::cout << (*it)->se3CfromW.get_rotation().inverse() << " " << (*it)->mse3GfromC.get_rotation() << std::endl;
    nBundleID = b.AddCamera((*it)->se3CfromW, true, (*it)->mse3PdfFfromC);
    mView_BundleID[*it] = nBundleID;
    mBundleID_View[nBundleID] = *it;
  }
  
  // Add the points' 3D position
  for(set<boost::shared_ptr<MapPoint> >::iterator it = sMapPoints.begin(); it!=sMapPoints.end(); it++)
  {
    int nBundleID = b.AddPoint((*it)->v3WorldPos, (*it)->bFixed);
    mPoint_BundleID[*it] = nBundleID;
    mBundleID_Point[nBundleID] = *it;
  }
  
  // Add the relevant point-in-keyframe measurements
  for(map<boost::shared_ptr<KeyFrame>, int>::iterator it = mView_BundleID.begin(); it!=mView_BundleID.end(); it++)
  {
    for(std::map<boost::shared_ptr<MapPoint>, Measurement>::iterator jt = it->first->mMeasurements.begin(); jt!= it->first->mMeasurements.end(); jt++)
      if(mPoint_BundleID.count(jt->first) != 0)
    	  b.AddMeas(it->second, mPoint_BundleID[jt->first], jt->second.v2RootPos, LevelScale(jt->second.nLevel) * LevelScale(jt->second.nLevel));
  }

  // Run the bundle adjuster. This returns the number of successful iterations
  uint64_t start_time = ClockGetTime();
  int nAccepted = b.Compute(&mbBundleAbortRequested, bGlobalSE3, bGlobalSO3);
  uint64_t end_time = ClockGetTime();
  if(!bRecent)
    ofsLogger << ClockGetTime() << " " << ClockTimeDifference(start_time, end_time)/1000 << " " << sAdjustSet.size() << " " << sFixedSet.size() << " " << sMapPoints.size() << " " << pMap->mrtAllKeyFrames.Size() << " " << pMap->mvAllMapPoints.size() << std::endl;
  
  if(nAccepted < 0)
  {
    // Crap: - LM Ran into a serious problem!
    // This is probably because the initial stereo was messed up.
    // Get rid of this map and start again! 
    /*cout << "!! MapMaker: Cholesky failure in bundle adjust. " << endl
   << "   The map is probably corrupt: Ditching the map. " << endl;
    mbResetRequested = true;
    return;*/
  }

  // Bundle adjustment did some updates, apply these to the map
  if(nAccepted > 0)
  {
    boost::unique_lock< boost::shared_mutex > lockMapPoints(pMap->mMapPointsLock);
    boost::unique_lock< boost::shared_mutex > lockKeyFrames(pMap->mKeyFramesLock);

    for(map<boost::shared_ptr<MapPoint>,int>::iterator itr = mPoint_BundleID.begin(); itr!=mPoint_BundleID.end(); itr++)
      itr->first->v3WorldPos = b.GetPoint(itr->second);
   
    for(map<boost::shared_ptr<KeyFrame>,int>::iterator itr = mView_BundleID.begin(); itr!=mView_BundleID.end(); itr++)
    {
      Vector<3> v3KFold_CamPos = itr->first->se3CfromW.inverse().get_translation();
      itr->first->se3CfromW = b.GetCamera(itr->second);
      Vector<3> v3KFnew_CamPos = itr->first->se3CfromW.inverse().get_translation();

      bool ok = pMap->mrtAllKeyFrames.ReInsert(v3KFold_CamPos.get_data_ptr(), v3KFnew_CamPos.get_data_ptr(), itr->first);
      if(!ok) std::cout << "Re-insertion failed" << std::endl;
    }

    if(bRecent)
      mbBundleConverged_Recent = false;
    mbBundleConverged_Full = false;

    lockMapPoints.unlock();
    lockKeyFrames.unlock();
  };
  
  if(b.Converged())
  {
    mbBundleConverged_Recent = true;
    if(!bRecent)
      mbBundleConverged_Full = true;
  }
  
  mbBundleRunning = false;
  mbBundleAbortRequested = false;
  
  // Handle outlier measurements:
  boost::unique_lock< boost::shared_mutex > lockMapPoints(pMap->mMapPointsLock);
  boost::unique_lock< boost::shared_mutex > lockKeyFrames(pMap->mKeyFramesLock);
  vector<pair<int,int> > vOutliers_PC_pair = b.GetOutlierMeasurements();
  for(unsigned int i=0; i<vOutliers_PC_pair.size(); i++)
  {
    boost::shared_ptr<MapPoint> &pp = mBundleID_Point[vOutliers_PC_pair[i].first];
    boost::shared_ptr<KeyFrame> pk = mBundleID_View[vOutliers_PC_pair[i].second];
    Measurement &m = pk->mMeasurements[pp];
    if(pp->bFixed) // Cannot get rid of fixed mappoints!
      continue;
    else if(pp->GoodMeasCount() <= 2 || m.Source == Measurement::SRC_ROOT)   // Is the original source kf considered an outlier? That's bad.
      pp->bBad = true;
    else
    {
      //Do we retry it? Depends where it came from!!
      if(m.Source == Measurement::SRC_TRACKER || m.Source == Measurement::SRC_EPIPOLAR)
        mvFailureQueue.push_back(pair<boost::shared_ptr<KeyFrame>,boost::shared_ptr<MapPoint> >(pk,pp));
      else
	      pp->sNeverRetryKFs.insert(pk);

	    pk->mMeasurements.erase(pp);
	    pp->sMeasurementKFs.erase(pk);

      /*// Normal of point is approximated by averaging optical axis directions of keyframes in which point is visible
      pp->v3Normal2 = makeVector(0, 0, 0);
      unsigned int nCount = 0;
      for(std::set<boost::shared_ptr<KeyFrame> >::iterator it = pp->sMeasurementKFs.begin(); it != pp->sMeasurementKFs.end(); ++it)
      {
        pp->v3Normal2 += unit((*it)->se3CfromW.inverse().get_translation() - pp->v3WorldPos);
        nCount++;
      }
      pp->v3Normal2 /= nCount;*/
    }
  }

  // Mark outlier Keyframes:
  vector<int> vOutlierKeyFrames = b.GetOutlierKeyFrames();
  for(unsigned int i=0; i<vOutlierKeyFrames.size(); i++)
  {
    boost::shared_ptr<KeyFrame> pk = mBundleID_View[vOutlierKeyFrames[i]];
    pk->nOutlierCount++;
  }

  lockMapPoints.unlock();
  lockKeyFrames.unlock();
}

// Mapmaker's try-to-find-a-point-in-a-keyframe code. This is used to update
// data association if a bad measurement was detected, or if a point
// was never searched for in a keyframe in the first place. This operates
// much like the tracker! So most of the code looks just like in 
// TrackerData.h.
bool MapMaker::ReFind_Common(boost::shared_ptr<KeyFrame> k, boost::shared_ptr<MapPoint> &p)
{
  // abort if either a measurement is already in the map, or we've
  // decided that this point-kf combo is beyond redemption
  if(p->sMeasurementKFs.count(k) || p->sNeverRetryKFs.count(k))
    return false;
  
  static PatchFinder Finder;
  Vector<3> v3Cam = k->se3CfromW*p->v3WorldPos;
  if(v3Cam[2] < 0.001)
    {
      p->sNeverRetryKFs.insert(k);
      return false;
    }
  Vector<2> v2ImPlane = project(v3Cam);
  if(v2ImPlane* v2ImPlane > mCamera.LargestRadiusInImage() * mCamera.LargestRadiusInImage())
    {
      p->sNeverRetryKFs.insert(k);
      return false;
    }
  
  Vector<2> v2Image = mCamera.Project(v2ImPlane);
  if(mCamera.Invalid())
    {
      p->sNeverRetryKFs.insert(k);
      return false;
    }

  ImageRef irImageSize = k->aLevels[0].im.size();
  if(v2Image[0] < 0 || v2Image[1] < 0 || v2Image[0] > irImageSize[0] || v2Image[1] > irImageSize[1])
    {
      p->sNeverRetryKFs.insert(k);
      return false;
    }
  
  Matrix<2> m2CamDerivs = mCamera.GetProjectionDerivs();
  Finder.MakeTemplateCoarse(p, k->se3CfromW, m2CamDerivs);
  
  if(Finder.TemplateBad())
    {
      p->sNeverRetryKFs.insert(k);
      return false;
    }
  
  bool bFound = Finder.FindPatchCoarse(ir(v2Image), *k, 4);  // Very tight search radius!
  if(!bFound)
    {
      p->sNeverRetryKFs.insert(k);
      return false;
    }
  
  // If we found something, generate a measurement struct and put it in the map
  Measurement m;
  m.nLevel = Finder.GetLevel();
  m.Source = Measurement::SRC_REFIND;
  
  if(Finder.GetLevel() > 0)
    {
      Finder.MakeSubPixTemplate();
      Finder.IterateSubPixToConvergence(*k,8);
      m.v2RootPos = Finder.GetSubPixPos();
      m.bSubPix = true;
    }
  else
    {
      m.v2RootPos = Finder.GetCoarsePosAsVector();
      m.bSubPix = false;
    };
  
  if(k->mMeasurements.count(p))
  {
    assert(0); // This should never happen, we checked for this at the start.
  }
  k->mMeasurements[p] = m;
  p->sMeasurementKFs.insert(k);

  // Normal of point is approximated by averaging optical axis directions of keyframes in which point is visible
  /*p->v3Normal2 = makeVector(0, 0, 0);
  unsigned int nCount = 0;
  for(std::set<boost::shared_ptr<KeyFrame> >::iterator it = p->sMeasurementKFs.begin(); it != p->sMeasurementKFs.end(); ++it)
  {
    p->v3Normal2 += unit((*it)->se3CfromW.inverse().get_translation() - p->v3WorldPos);
    nCount++;
  }
  p->v3Normal2 /= nCount;*/

  return true;
}

// A general data-association update for a single keyframe
// Do this on a new key-frame when it's passed in by the tracker
int MapMaker::ReFindInSingleKeyFrame(boost::shared_ptr<KeyFrame> k)
{
  vector<boost::shared_ptr<MapPoint> > vToFind;
  for(std::set<boost::shared_ptr<MapPoint> >::iterator it = mMap->msLocalMapPoints.begin(); it != mMap->msLocalMapPoints.end(); ++it)
    vToFind.push_back(*it);
  
  int nFoundNow = 0;
  for(unsigned int i=0; i<vToFind.size(); i++)
    if(ReFind_Common(k,vToFind[i]))
      nFoundNow++;

  return nFoundNow;
};

// When new map points are generated, they're only created from a stereo pair
// this tries to make additional measurements in other KFs which they might
// be in.
void MapMaker::ReFindNewlyMade()
{
  if(mvNewQueue.empty())
    return;

  while(!mvNewQueue.empty() && mvpKeyFrameQueue.size() == 0)
  {
    boost::shared_ptr<MapPoint> pNew = mvNewQueue.back();
    if(!(pNew->bBad))
    {
      for(std::set<boost::shared_ptr<KeyFrame> >::iterator it = mMap->msLocalOptimKeyFrames.begin(); it != mMap->msLocalOptimKeyFrames.end(); ++it)
        ReFind_Common(*it, pNew);
    }
    mvNewQueue.pop_back();
  }
};

// Dud measurements get a second chance.
void MapMaker::ReFindFromFailureQueue()
{
  if(mvFailureQueue.size() == 0)
    return;
  sort(mvFailureQueue.begin(), mvFailureQueue.end());
  vector<pair<boost::shared_ptr<KeyFrame>, boost::shared_ptr<MapPoint> > >::iterator it;
  int nFound=0;
  for(it = mvFailureQueue.begin(); it!=mvFailureQueue.end(); it++)
    if(ReFind_Common(it->first, it->second))
      nFound++;
  
  mvFailureQueue.erase(mvFailureQueue.begin(), it);
};

// Is the tracker's camera pose in cloud-cuckoo land?
bool MapMaker::IsDistanceToNearestKeyFrameExcessive(boost::shared_ptr<KeyFrame> kCurrent)
{
  return DistToNearestKeyFrame(kCurrent) > mdKeyFrameSpacing * 10.0;
}

// Is the tracker's camera pose in cloud-cuckoo land?
bool MapMaker::IsDistanceToNearestKeyFrameExcessive(double dDist)
{
  return dDist > mdKeyFrameSpacing * 10.0;
}


// Calculates the depth(z-) distribution of map points visible in a keyframe
// This function is only used for the first two keyframes - all others
// get this filled in by the tracker
bool MapMaker::RefreshSceneDepth(boost::shared_ptr<KeyFrame> pKF)
{
  double dSumDepth = 0.0;
  double dSumDepthSquared = 0.0;
  int nMeas = 0;

  for(std::map<boost::shared_ptr<MapPoint>, Measurement>::iterator it = pKF->mMeasurements.begin(); it!=pKF->mMeasurements.end(); it++)
  {
    MapPoint &point = *it->first;
    Vector<3> v3PosK = pKF->se3CfromW * point.v3WorldPos;
    if(v3PosK[2] < 0.001 || v3PosK[2] > 1000.0)
      continue;

    dSumDepth += v3PosK[2];
    dSumDepthSquared += v3PosK[2] * v3PosK[2];
    nMeas++;
  }

  if ( nMeas <= 2) {
    return false;
  }

  pKF->dSceneDepthMean = dSumDepth / nMeas;
  pKF->dSceneDepthSigma = sqrt((dSumDepthSquared / nMeas) - (pKF->dSceneDepthMean) * (pKF->dSceneDepthMean));
  return true;
}

// Compute and draw a mesh of the surface using adaptive compactly supported radial basis functions (ACSRBF)
void MapMaker::ComputeMesh()
{
  static gvar3<double> gvdMinPatchAngle("MapMaker.MinPatchAngle", 2.3562, SILENT); //135 degrees
  static gvar3<int> gvnMinPatchArea("MapMaker.MinPatchArea", 100, SILENT);  //Projected patch must have an area of at least 100 pixels
  static gvar3<int> gvnMaxSSDPerPixel("MapMaker.MaxSSDPerPixel", 3000, SILENT);//300
  static gvar3<int> gvnMinPointsRBF("MapMaker.MinPointsRBF", 100, SILENT);
  static gvar3<int> gvnMinPointsBloomenthal("MapMaker.MinPointsBloomenthal", 20, SILENT);//100
  static gvar3<double> gvdSmoothRBF("MapMaker.SmoothRBF", 0.2, SILENT);
  static gvar3<double> gvdErrorRBF("MapMaker.ErrorRBF", 0.1, SILENT);

  PointSet* ps = new PointSet();
  ps->allocateStorage(mMap->msLocalMapPoints.size()); //Allocate maximum storage

  for(std::set<boost::shared_ptr<MapPoint> >::iterator it = mMap->msLocalMapPoints.begin(); it != mMap->msLocalMapPoints.end(); ++it)
  {
    MapPoint &p= **it; 

    if(!p.bBad && p.nMEstimatorInlierCount > 20 && 10*p.nMEstimatorOutlierCount < p.nMEstimatorInlierCount)//p.sMeasurementKFs.size() > 4)//  
    {
      // Normal of point is approximated by averaging optical axis directions of keyframes in which point is visible
      Vector<3> v3AvgDir = makeVector(0, 0, 0);
      unsigned int nCount = 0;
      for(std::set<boost::shared_ptr<KeyFrame> >::iterator it2 = p.sMeasurementKFs.begin(); it2 != p.sMeasurementKFs.end(); ++it2)
      {
        Vector<3> v3CamPosition = (*it2)->se3CfromW.inverse().get_translation();
        Vector<3> v3Normal = unit(v3CamPosition - p.v3WorldPos);
        v3AvgDir += v3Normal;
        nCount++;
      }
      v3AvgDir /= nCount;

      ps->addPoint(p.v3WorldPos[0], p.v3WorldPos[1], p.v3WorldPos[2], -v3AvgDir[0], -v3AvgDir[1], -v3AvgDir[2]);
    }
  }

  if(ps->getNumberPoints() > *gvnMinPointsRBF)
  {  
    boost::unique_lock< boost::shared_mutex > lockMesh(mMap->mMeshLock);
    double dGridSize = mMap->mesh.GetGridSize();
    RBF *rbf = MultiScaleRBF(ps, *gvdSmoothRBF, *gvdErrorRBF);
    rbf->setSupportConstraint(*gvnMinPointsBloomenthal);

	  Bloomenthal bloom;
    Bloomenthal::VERTICES vertexlist;
    Bloomenthal::CUBELIST* cubelist;
    double dOriginX = rbf->getPointSet()->getPointX(0);
    double dOriginY = rbf->getPointSet()->getPointY(0);
    double dOriginZ = rbf->getPointSet()->getPointZ(0);

	  bloom.setImplicitFunction(rbf);
    int nCubeCount = bloom.polygonize(vertexlist, cubelist, dOriginX, dOriginY, dOriginZ, dGridSize);
    //std::cout << "Cube count: " << nCubeCount << std::endl;

    // Offset of this mesh relative to existing mesh with origin at (0,0,0)
    int nOriginCi = floor(dOriginX/dGridSize);
    int nOriginCj = floor(dOriginY/dGridSize);
    int nOriginCk = floor(dOriginZ/dGridSize);

    mMap->mesh.Reset(); //TODO: Do not reset!!!

    //Pass through and match boundary vertices with existing mesh
	  Bloomenthal::CUBELIST *ccube = cubelist;

	  /*for(int i = 0; i < nCubeCount; i++)
    {
      Bloomenthal::TRILIST *ctri = ccube->tris;
      std::vector<Cube*> cubes;
      int count = mMap->mesh.GetCubes(nOriginCi+ccube->ci-1, nOriginCj+ccube->cj-1, nOriginCk+ccube->ck-1, nOriginCi+ccube->ci+1, nOriginCj+ccube->cj+1, nOriginCk+ccube->ck+1, cubes);

      if(count == 0)
      {
        for(int j = 0; j < ccube->triscount; j++)
        {
          ctri->b1 = ctri->b2 = ctri->b3 = false;
          ctri = ctri->next;
        }
      }
      else
      {
        for(int j = 0; j < ccube->triscount; j++)
        {
          if(ctri->b1 || ctri->b2 || ctri->b3) //A face vertex lies on the boundary
          {
            for(int k = 0; k < cubes.size(); ++k)
            {
              for(int l = 0; l < cubes[k]->faces.size(); ++l)
              {
                if(ctri->b1)
                {
                  if(comp(cubes[k]->faces[l].x1, vertexlist.ptr[ctri->i1].position.x) && comp(cubes[k]->faces[l].y1, vertexlist.ptr[ctri->i1].position.y))
                  {
                    vertexlist.ptr[ctri->i1].position.z = cubes[k]->faces[l].z1;
                    ctri->b1 = false;
                  }
                  else if(comp(cubes[k]->faces[l].x1, vertexlist.ptr[ctri->i1].position.x) && comp(cubes[k]->faces[l].z1, vertexlist.ptr[ctri->i1].position.z))
                  {
                    vertexlist.ptr[ctri->i1].position.y = cubes[k]->faces[l].y1;
                    ctri->b1 = false;
                  }
                  else if(comp(cubes[k]->faces[l].y1, vertexlist.ptr[ctri->i1].position.y) && comp(cubes[k]->faces[l].z1, vertexlist.ptr[ctri->i1].position.z))
                  {
                    vertexlist.ptr[ctri->i1].position.x = cubes[k]->faces[l].x1;
                    ctri->b1 = false;
                  }
                  else if(comp(cubes[k]->faces[l].x2, vertexlist.ptr[ctri->i1].position.x) && comp(cubes[k]->faces[l].y2, vertexlist.ptr[ctri->i1].position.y))
                  {
                    vertexlist.ptr[ctri->i1].position.z = cubes[k]->faces[l].z2;
                    ctri->b1 = false;
                  }
                  else if(comp(cubes[k]->faces[l].x2, vertexlist.ptr[ctri->i1].position.x) && comp(cubes[k]->faces[l].z2, vertexlist.ptr[ctri->i1].position.z))
                  {
                    vertexlist.ptr[ctri->i1].position.y = cubes[k]->faces[l].y2;
                    ctri->b1 = false;
                  }
                  else if(comp(cubes[k]->faces[l].y2, vertexlist.ptr[ctri->i1].position.y) && comp(cubes[k]->faces[l].z2, vertexlist.ptr[ctri->i1].position.z))
                  {
                    vertexlist.ptr[ctri->i1].position.x = cubes[k]->faces[l].x2;
                    ctri->b1 = false;
                  }
                  else if(comp(cubes[k]->faces[l].x3, vertexlist.ptr[ctri->i1].position.x) && comp(cubes[k]->faces[l].y3, vertexlist.ptr[ctri->i1].position.y))
                  {
                    vertexlist.ptr[ctri->i1].position.z = cubes[k]->faces[l].z3;
                    ctri->b1 = false;
                  }
                  else if(comp(cubes[k]->faces[l].x3, vertexlist.ptr[ctri->i1].position.x) && comp(cubes[k]->faces[l].z3, vertexlist.ptr[ctri->i1].position.z))
                  {
                    vertexlist.ptr[ctri->i1].position.y = cubes[k]->faces[l].y3;
                    ctri->b1 = false;
                  }
                  else if(comp(cubes[k]->faces[l].y3, vertexlist.ptr[ctri->i1].position.y) && comp(cubes[k]->faces[l].z3, vertexlist.ptr[ctri->i1].position.z))
                  {
                    vertexlist.ptr[ctri->i1].position.x = cubes[k]->faces[l].x3;
                    ctri->b1 = false;
                  }
                }

                if(ctri->b2)
                {
                  if(comp(cubes[k]->faces[l].x1, vertexlist.ptr[ctri->i2].position.x) && comp(cubes[k]->faces[l].y1, vertexlist.ptr[ctri->i2].position.y))
                  {
                    vertexlist.ptr[ctri->i2].position.z = cubes[k]->faces[l].z1;
                    ctri->b2 = false;
                  }
                  else if(comp(cubes[k]->faces[l].x1, vertexlist.ptr[ctri->i2].position.x) && comp(cubes[k]->faces[l].z1, vertexlist.ptr[ctri->i2].position.z))
                  {
                    vertexlist.ptr[ctri->i2].position.y = cubes[k]->faces[l].y1;
                    ctri->b2 = false;
                  }
                  else if(comp(cubes[k]->faces[l].y1, vertexlist.ptr[ctri->i2].position.y) && comp(cubes[k]->faces[l].z1, vertexlist.ptr[ctri->i2].position.z))
                  {
                    vertexlist.ptr[ctri->i2].position.x = cubes[k]->faces[l].x1;
                    ctri->b2 = false;
                  }
                  else if(comp(cubes[k]->faces[l].x2, vertexlist.ptr[ctri->i2].position.x) && comp(cubes[k]->faces[l].y2, vertexlist.ptr[ctri->i2].position.y))
                  {
                    vertexlist.ptr[ctri->i2].position.z = cubes[k]->faces[l].z2;
                    ctri->b2 = false;
                  }
                  else if(comp(cubes[k]->faces[l].x2, vertexlist.ptr[ctri->i2].position.x) && comp(cubes[k]->faces[l].z2, vertexlist.ptr[ctri->i2].position.z))
                  {
                    vertexlist.ptr[ctri->i2].position.y = cubes[k]->faces[l].y2;
                    ctri->b2 = false;
                  }
                  else if(comp(cubes[k]->faces[l].y2, vertexlist.ptr[ctri->i2].position.y) && comp(cubes[k]->faces[l].z2, vertexlist.ptr[ctri->i2].position.z))
                  {
                    vertexlist.ptr[ctri->i2].position.x = cubes[k]->faces[l].x2;
                    ctri->b2 = false;
                  }
                  else if(comp(cubes[k]->faces[l].x3, vertexlist.ptr[ctri->i2].position.x) && comp(cubes[k]->faces[l].y3, vertexlist.ptr[ctri->i2].position.y))
                  {
                    vertexlist.ptr[ctri->i2].position.z = cubes[k]->faces[l].z3;
                    ctri->b2 = false;
                  }
                  else if(comp(cubes[k]->faces[l].x3, vertexlist.ptr[ctri->i2].position.x) && comp(cubes[k]->faces[l].z3, vertexlist.ptr[ctri->i2].position.z))
                  {
                    vertexlist.ptr[ctri->i2].position.y = cubes[k]->faces[l].y3;
                    ctri->b2 = false;
                  }
                  else if(comp(cubes[k]->faces[l].y3, vertexlist.ptr[ctri->i2].position.y) && comp(cubes[k]->faces[l].z3, vertexlist.ptr[ctri->i2].position.z))
                  {
                    vertexlist.ptr[ctri->i2].position.x = cubes[k]->faces[l].x3;
                    ctri->b2 = false;
                  }
                }

                if(ctri->b3)
                {
                  if(comp(cubes[k]->faces[l].x1, vertexlist.ptr[ctri->i3].position.x) && comp(cubes[k]->faces[l].y1, vertexlist.ptr[ctri->i3].position.y))
                  {
                    vertexlist.ptr[ctri->i3].position.z = cubes[k]->faces[l].z1;
                    ctri->b3 = false;
                  }
                  else if(comp(cubes[k]->faces[l].x1, vertexlist.ptr[ctri->i3].position.x) && comp(cubes[k]->faces[l].z1, vertexlist.ptr[ctri->i3].position.z))
                  {
                    vertexlist.ptr[ctri->i3].position.y = cubes[k]->faces[l].y1;
                    ctri->b3 = false;
                  }
                  else if(comp(cubes[k]->faces[l].y1, vertexlist.ptr[ctri->i3].position.y) && comp(cubes[k]->faces[l].z1, vertexlist.ptr[ctri->i3].position.z))
                  {
                    vertexlist.ptr[ctri->i3].position.x = cubes[k]->faces[l].x1;
                    ctri->b3 = false;
                  }
                  else if(comp(cubes[k]->faces[l].x2, vertexlist.ptr[ctri->i3].position.x) && comp(cubes[k]->faces[l].y2, vertexlist.ptr[ctri->i3].position.y))
                  {
                    vertexlist.ptr[ctri->i3].position.z = cubes[k]->faces[l].z2;
                    ctri->b3 = false;
                  }
                  else if(comp(cubes[k]->faces[l].x2, vertexlist.ptr[ctri->i3].position.x) && comp(cubes[k]->faces[l].z2, vertexlist.ptr[ctri->i3].position.z))
                  {
                    vertexlist.ptr[ctri->i3].position.y = cubes[k]->faces[l].y2;
                    ctri->b3 = false;
                  }
                  else if(comp(cubes[k]->faces[l].y2, vertexlist.ptr[ctri->i3].position.y) && comp(cubes[k]->faces[l].z2, vertexlist.ptr[ctri->i3].position.z))
                  {
                    vertexlist.ptr[ctri->i3].position.x = cubes[k]->faces[l].x2;
                    ctri->b3 = false;
                  }
                  else if(comp(cubes[k]->faces[l].x3, vertexlist.ptr[ctri->i3].position.x) && comp(cubes[k]->faces[l].y3, vertexlist.ptr[ctri->i3].position.y))
                  {
                    vertexlist.ptr[ctri->i3].position.z = cubes[k]->faces[l].z3;
                    ctri->b3 = false;
                  }
                  else if(comp(cubes[k]->faces[l].x3, vertexlist.ptr[ctri->i3].position.x) && comp(cubes[k]->faces[l].z3, vertexlist.ptr[ctri->i3].position.z))
                  {
                    vertexlist.ptr[ctri->i3].position.y = cubes[k]->faces[l].y3;
                    ctri->b3 = false;
                  }
                  else if(comp(cubes[k]->faces[l].y3, vertexlist.ptr[ctri->i3].position.y) && comp(cubes[k]->faces[l].z3, vertexlist.ptr[ctri->i3].position.z))
                  {
                    vertexlist.ptr[ctri->i3].position.x = cubes[k]->faces[l].x3;
                    ctri->b3 = false;
                  }
                }

                if(!ctri->b1 && !ctri->b2 && !ctri->b3) //All face vertices on the boundary have been adjusted
                  break;
              }
            }
          }
        }
        ctri = ctri->next;
      }
      ccube = ccube->next;
	  }*/

    //Pass through and eliminate any overlap with existing mesh
	  ccube = cubelist;
	  for(int i = 0; i < nCubeCount; i++)
    {
      mMap->mesh.RemoveCubes(nOriginCi + ccube->ci, nOriginCj + ccube->cj, nOriginCk + ccube->ck - 1000, nOriginCi + ccube->ci, nOriginCj + ccube->cj, nOriginCk + ccube->ck + 1000); //Remove all existing faces in this cube
      ccube = ccube->next;
    }

    //Pass through insert new mesh faces
	  ccube = cubelist;
	  for(int i = 0; i < nCubeCount; i++)
    {
      boost::shared_ptr<Cube> newcube(new Cube());

      Bloomenthal::TRILIST *ctri = ccube->tris;
      for(int j = 0; j < ccube->triscount; j++)
      {
        Vector<3> v3Vertex0 = makeVector(vertexlist.ptr[ctri->i1].position.x, vertexlist.ptr[ctri->i1].position.y, vertexlist.ptr[ctri->i1].position.z);
        Vector<3> v3Vertex1 = makeVector(vertexlist.ptr[ctri->i2].position.x, vertexlist.ptr[ctri->i2].position.y, vertexlist.ptr[ctri->i2].position.z);
        Vector<3> v3Vertex2 = makeVector(vertexlist.ptr[ctri->i3].position.x, vertexlist.ptr[ctri->i3].position.y, vertexlist.ptr[ctri->i3].position.z);
        Vector<3> v3Normal = unit((v3Vertex1 - v3Vertex0) ^ (v3Vertex2 - v3Vertex0));  //outward pointing normal
        Vector<3> v3Center = makeVector((v3Vertex0[0]+v3Vertex1[0]+v3Vertex2[0])/3,(v3Vertex0[1]+v3Vertex1[1]+v3Vertex2[1])/3,(v3Vertex0[2]+v3Vertex1[2]+v3Vertex2[2])/3);
       
        newcube->faces.push_back(new Face(vertexlist.ptr[ctri->i1].position.x, vertexlist.ptr[ctri->i1].position.y, vertexlist.ptr[ctri->i1].position.z,
        vertexlist.ptr[ctri->i2].position.x, vertexlist.ptr[ctri->i2].position.y, vertexlist.ptr[ctri->i2].position.z,
        vertexlist.ptr[ctri->i3].position.x, vertexlist.ptr[ctri->i3].position.y, vertexlist.ptr[ctri->i3].position.z, 
        ctri->b1, ctri->b2, ctri->b3, 
        v3Center[0], v3Center[1], v3Center[2],
        v3Normal[0], v3Normal[1], v3Normal[2]));

        for(std::set<boost::shared_ptr<KeyFrame> >::iterator it = mMap->msLocalOptimKeyFrames.begin(); it != mMap->msLocalOptimKeyFrames.end(); ++it)
        {
          Vector<3> v3Cam0 = (*it)->se3CfromW * v3Vertex0;
          Vector<3> v3Cam1 = (*it)->se3CfromW * v3Vertex1;
          Vector<3> v3Cam2 = (*it)->se3CfromW * v3Vertex2;

          if(v3Cam0[2] < 0.001 || v3Cam1[2] < 0.001 || v3Cam2[2] < 0.001)
            continue;

          //Compute angle between patch normal and keyframe optical axis
          Vector<3> v3CamNormal = (*it)->se3CfromW.inverse().get_rotation() * makeVector(0,0,1);
          double angle = acos(v3Normal * v3CamNormal); 

          if(angle < *gvdMinPatchAngle)
            continue;

          Vector<2> v2ImPlane0 = project(v3Cam0);
          Vector<2> v2ImPlane1 = project(v3Cam1);
          Vector<2> v2ImPlane2 = project(v3Cam2);

          if(v2ImPlane0*v2ImPlane0 > mdLargestRadiusInImage || v2ImPlane1*v2ImPlane1 > mdLargestRadiusInImage || v2ImPlane2*v2ImPlane2 > mdLargestRadiusInImage)
            continue;

          Vector<2> v2ImagePix0 = mCamera.Project(v2ImPlane0);
          if(mCamera.Invalid() || v2ImagePix0[0] < 0 || v2ImagePix0[1] < 0 || v2ImagePix0[0] > mv2ImageSize[0] || v2ImagePix0[1] > mv2ImageSize[1])
            continue;

          Vector<2> v2ImagePix1 = mCamera.Project(v2ImPlane1);
          if(mCamera.Invalid() || v2ImagePix1[0] < 0 || v2ImagePix1[1] < 0 || v2ImagePix1[0] > mv2ImageSize[0] || v2ImagePix1[1] > mv2ImageSize[1])
            continue;

          Vector<2> v2ImagePix2 = mCamera.Project(v2ImPlane2);
          if(mCamera.Invalid() || v2ImagePix2[0] < 0 || v2ImagePix2[1] < 0 || v2ImagePix2[0] > mv2ImageSize[0] || v2ImagePix2[1] > mv2ImageSize[1])
            continue;

          double area = 0.5*fabs((v2ImagePix1[0]-v2ImagePix0[0])*(v2ImagePix2[1]-v2ImagePix0[1]) - (v2ImagePix2[0]-v2ImagePix0[0])*(v2ImagePix1[1]-v2ImagePix0[1]));

          if(area < *gvnMinPatchArea)
            continue;

          newcube->faces.back()->patches.push_back(new Patch((*it)->gluTextureId, v2ImagePix0[0], v2ImagePix0[1], v2ImagePix1[0], v2ImagePix1[1], v2ImagePix2[0], v2ImagePix2[1]));//, (void*)(*it)
        }

        //Check face has at least one patch...
        if(newcube->faces.back()->patches.size() == 0)
          newcube->faces.pop_back();

        ctri = ctri->next;
	    }

      //Check cube has at least one face...
      if(newcube->faces.size() == 0)
        newcube.reset();
      else
        mMap->mesh.SetCube(nOriginCi+ccube->ci, nOriginCj+ccube->cj, nOriginCk+ccube->ck, newcube);

      ccube = ccube->next;
    }

    ccube = cubelist;
	  for(int i = 0; i < nCubeCount; i++)
    {
      Bloomenthal::TRILIST *ctri = ccube->tris;
      for(int j = 0; j < ccube->triscount; j++)
      {
		    Bloomenthal::TRILIST* previous = ctri;
		    ctri = ctri->next;
		    delete previous;
      }
      Bloomenthal::CUBELIST* pcube = ccube;
      ccube = ccube->next;
      delete pcube;
    }

	  if (vertexlist.ptr != NULL) 
      free((char *)vertexlist.ptr);

    delete rbf;
    lockMesh.unlock();
  }

  delete ps;
}

bool MapMaker::comp(float a, float b)
{
  if (fabs(a - b) < 0.00001)
    return true;
  else
    return false;
}

RBF* MapMaker::MultiScaleRBF(PointSet *ps, const float smooth, const float error) 
{
	//Estimation of Final Level
  RBF *rbf = new RBF;
	rbf->setPointSet(ps, false);
	float peak = rbf->getOctreePeak(); 
	int M = -(int)(log(rbf->getOctreeAveragedLeafSize()*2.0/(peak*3.0))/log(2.0)) + 1;
	float support = 1.5f*peak;
	float final = support*pow(2.0, -M);

	//increase child 
	rbf->splitOctreeChild(M);

	//assign cell points by averaging
	rbf->computeOctreeCenter();

	RBF** rbfi = new RBF*[M+1];
	PointSet** qs = new PointSet*[M+1];

	for(int k=0; k<=M; k++)
  {
		int size;
    if(k == M)
      size = ps->getNumberPoints();
    else
      rbf->generateOctreePointList(size, 0, 0, 0, 1000000, k);//10000000000

		qs[k] = new PointSet;
		qs[k]->allocateStorage(size);

    if(k == M)
    {
      if(error != 0)
      {
        //int dropped = 0;
		    for(int i=0; i < size; i++)
        {
			    float g[3], p[3] = {ps->getPointX(i), ps->getPointY(i), ps->getPointZ(i)};
			    float f = rbfi[k-1]->valueAndGradient(g, p[0], p[1], p[2]);
			    float len = sqrt(g[0]*g[0] + g[1]*g[1] + g[2]*g[2]);
			    float dist; 
			    if(len != 0)
				    dist = fabs(f)/len;
			    else
				    dist = 10000000;

			    if(dist < final*error && (1.0 - (ps->getNormalX(i)*g[0] + ps->getNormalY(i)*g[1] + ps->getNormalZ(i)*g[2]))/len < 0.05)
				    continue;//dropped++;//
			
			    /*if(len != 0){
				    if(f > 0){
					    p[0] -= final*error*g[0]/len;
					    p[1] -= final*error*g[1]/len;
					    p[2] -= final*error*g[2]/len;
				    }
				    else{
					    p[0] += final*error*g[0]/len;
					    p[1] += final*error*g[1]/len;
					    p[2] += final*error*g[2]/len;
				    }
			    }*/

		      qs[k]->addPoint(p[0], p[1], p[2], ps->getNormalX(i), ps->getNormalY(i), ps->getNormalZ(i));
		    }
        //std::cout << M << " dropped: " << dropped << "/" << size << std::endl;
	    }
      else
      {
	      for(int i=0; i<size; i++)
		      qs[k]->addPoint(ps->getPointX(i), ps->getPointY(i), ps->getPointZ(i), ps->getNormalX(i), ps->getNormalY(i), ps->getNormalZ(i));
      }
    }
    else
    {
			if(error != 0 && k != 0)
      {
        //int dropped = 0;
				for(int i=0; i<size; i++)
        {
					float g[3];
					float f = rbfi[k-1]->valueAndGradient(g, rbf->getOctreePointX(i), rbf->getOctreePointY(i), rbf->getOctreePointZ(i));
					float len = sqrt(g[0]*g[0] + g[1]*g[1] + g[2]*g[2]);
					float dist; 
					if(len != 0)
						dist = (fabs(f)/len);
					else
						dist = 10000000;

					if(dist < final*error && (1.0 - (rbf->getOctreeNormalX(i)*g[0] + rbf->getOctreeNormalY(i)*g[1] + rbf->getOctreeNormalZ(i)*g[2]))/len < 0.05)
						continue;//dropped++;//

					qs[k]->addPoint(rbf->getOctreePointX(i), rbf->getOctreePointY(i), rbf->getOctreePointZ(i),
					                rbf->getOctreeNormalX(i), rbf->getOctreeNormalY(i), rbf->getOctreeNormalZ(i));
				}
        //std::cout << k << " dropped: " << dropped << "/" << size << std::endl;
			}
      else
      {
		    for(int i=0; i<size; i++)
        {
			    qs[k]->addPoint(rbf->getOctreePointX(i), rbf->getOctreePointY(i), rbf->getOctreePointZ(i),
			                    rbf->getOctreeNormalX(i), rbf->getOctreeNormalY(i), rbf->getOctreeNormalZ(i));
		    }
      }
    }

		rbfi[k] = new RBF;
		rbfi[k]->setPointSet(qs[k], true);

    if(k == 0)
    {
			rbfi[k]->setSupport(support);
			rbfi[k]->setGlobal();
    }
		else
    {
	    rbfi[k]->setSubRBF(rbfi[k-1]);
	    rbfi[k]->computeQuad(support); //Local fitting
      rbfi[k]->fit(support, 0.0f,smooth*k); 			//Global fitting
		}
		support *= 0.5f;
	}

  delete rbf; //delete original rbf

  return rbfi[M];
}

void MapMaker::GUICommandCallBack(void* ptr, string sCommand, string sParams)
{
  Command c;
  c.sCommand = sCommand;
  c.sParams = sParams;
  ((MapMaker*) ptr)->mvQueuedCommands.push_back(c);
}

// Called only by the callback func, can handle entire map
void MapMaker::GUICommandHandler(string sCommand, string sParams)
{
  if(sCommand=="SaveMap")
  {
    mMap->Save();
    return;
  }
  else if(sCommand=="TogglePause")
  {
    mbMapMakerPaused = !mbMapMakerPaused;
    return;
  }

  cout << "! MapMaker::GUICommandHandler: unhandled command "<< sCommand << endl;
  exit(1);
}; 

